package com.rockwell.robot2015;

import java.util.concurrent.Executor;
import java.util.concurrent.Executors;

import net.wimpi.modbus.usbserial.SerialParameters;

import com.rockwell.robot2015.modbus.ModbusSerialClient;
import com.rockwell.robot2015.model.ColorBlobInfo;
import com.rockwell.robot2015.model.GameStage;
import com.rockwell.robot2015.model.IRobotLogger;
import com.rockwell.robot2015.model.OnColorBlobDetectListener;
import com.rockwell.robot2015.model.OnOrientationChangeListener;
import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotConfig;
import com.rockwell.robot2015.model.command.RobotCommander;
import com.rockwell.robot2015.model.detectors.DropAreaDetector;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.detectors.Maze1Detector;
import com.rockwell.robot2015.model.detectors.Maze2Detector;
import com.rockwell.robot2015.model.detectors.Maze3Detector;
import com.rockwell.robot2015.model.detectors.MazeEntranceGuideDetector;
//import com.rockwell.robot2015.model.detectors.ObstacleDetector;
import com.rockwell.robot2015.model.detectors.OuterWallDetector;
import com.rockwell.robot2015.model.detectors.TreasureDetector;
import com.rockwell.robot2015.model.sensors.RobotSensors;
import com.rockwell.robot2015.model.solvers.LeftFirstMazeSolver;
import com.rockwell.robot2015.views.MazeCameraView;

import android.hardware.usb.UsbManager;
import android.os.AsyncTask;
import android.os.Bundle;
import android.app.Activity;
import android.content.Context;
import android.content.Intent;
import android.text.Editable;
import android.text.TextWatcher;
import android.view.Menu;
import android.view.View;
import android.view.View.OnClickListener;
import android.widget.Button;
import android.widget.ScrollView;
import android.widget.TextView;
import android.widget.Toast;

public class TestSequenceActivity extends Activity implements
		Robot.ISequenceTester {
	private GameStage m_gameStage;
	private Robot m_robot;
	private ModbusSerialClient m_modbusMaster;
	private MazeCameraView m_mazeCameraView;
	private Executor m_executor;

	boolean m_shouldStop = false;

	private IDetector m_treasureDetector;
	private IDetector m_obstacleDetector;
	private IDetector m_maze1Detector;
	private IDetector m_maze2Detector;
	private IDetector m_maze3Detector;
	private IDetector m_mazeEntranceGuideDetector;
	private IDetector m_outerWallDetector;
	private IDetector m_dropAreaDetector;
	
	private IRobotLogger m_logger = new IRobotLogger() {

		@Override
		public void log(final String msg) {
			TestSequenceActivity.this.runOnUiThread(new Runnable() {
				public void run() {
					// Toast.makeText(MaintenanceActivity.this, msg,
					// Toast.LENGTH_SHORT).show();
					TextView logDisplay = (TextView) findViewById(R.id.text_log);
					logDisplay.append(msg + "\n");
				}
			});
		}

		@Override
		public void log(final Exception ex) {
			TestSequenceActivity.this.runOnUiThread(new Runnable() {
				public void run() {
					Toast.makeText(TestSequenceActivity.this, ex.getMessage(),
							Toast.LENGTH_SHORT).show();
				}
			});
		}

		@Override
		public void setCurrentState(String state) {

		}

		@Override
		public void onRobotPaused() {
		}

		@Override
		public void onRobotResumed() {
		}

		@Override
		public void onRobotIdle() {
		}
	};

	private OnOrientationChangeListener m_orientationListener = new OnOrientationChangeListener() {
		public void onOrientationChanged(float[] orientation) {
			m_robot.setOrientation(orientation[0]);
		}
	};

	private OnColorBlobDetectListener m_treasureListener = new OnColorBlobDetectListener() {
		boolean enabled = true;

		@Override
		public boolean isEnabled() {
			return enabled;
		}

		@Override
		public void setEnabled(boolean enabled) {
			this.enabled = enabled;
		}
		
		public void onDetected(boolean present, ColorBlobInfo info, int length,
				int width) {
			m_treasureDetector.onDetect(present, info, length, width);
		}
	};

	private OnColorBlobDetectListener m_obstacleListener = new OnColorBlobDetectListener() {
		boolean enabled = true;

		@Override
		public boolean isEnabled() {
			return enabled;
		}

		@Override
		public void setEnabled(boolean enabled) {
			this.enabled = enabled;
		}
		
		public void onDetected(final boolean present, final ColorBlobInfo info,
				final int length, final int width) {
			m_obstacleDetector.onDetect(present, info, length, width);
		}
	};

	private OnColorBlobDetectListener m_mazeEntranceGuideListener = new OnColorBlobDetectListener() {
		boolean enabled = true;

		@Override
		public boolean isEnabled() {
			return enabled;
		}

		@Override
		public void setEnabled(boolean enabled) {
			this.enabled = enabled;
		}
		
		public void onDetected(final boolean present, final ColorBlobInfo info,
				final int length, final int width) {
			m_mazeEntranceGuideDetector.onDetect(present, info, length, width);
		}
	};

	private OnColorBlobDetectListener m_dropAreaListener = new OnColorBlobDetectListener() {
		boolean enabled = true;

		@Override
		public boolean isEnabled() {
			return enabled;
		}

		@Override
		public void setEnabled(boolean enabled) {
			this.enabled = enabled;
		}
		
		public void onDetected(final boolean present, final ColorBlobInfo info,
				final int length, final int width) {
			m_dropAreaDetector.onDetect(present, info, length, width);
		}
	};

	private OnColorBlobDetectListener m_outerWallListener = new OnColorBlobDetectListener() {
		boolean enabled = true;

		@Override
		public boolean isEnabled() {
			return enabled;
		}

		@Override
		public void setEnabled(boolean enabled) {
			this.enabled = enabled;
		}
		
		public void onDetected(final boolean present, final ColorBlobInfo info,
				final int length, final int width) {
			m_outerWallDetector.onDetect(present, info, length, width);
		}
	};

	private OnColorBlobDetectListener m_maze1Listener = new OnColorBlobDetectListener() {
		boolean enabled = true;

		@Override
		public boolean isEnabled() {
			return enabled;
		}

		@Override
		public void setEnabled(boolean enabled) {
			this.enabled = enabled;
		}
		
		public void onDetected(final boolean present, final ColorBlobInfo info,
				final int length, final int width) {
			m_maze1Detector.onDetect(present, info, length, width);
		}
	};

	private OnColorBlobDetectListener m_maze2Listener = new OnColorBlobDetectListener() {
		boolean enabled = true;

		@Override
		public boolean isEnabled() {
			return enabled;
		}

		@Override
		public void setEnabled(boolean enabled) {
			this.enabled = enabled;
		}
		
		public void onDetected(final boolean present, final ColorBlobInfo info,
				final int length, final int width) {
			m_maze2Detector.onDetect(present, info, length, width);
		}
	};

	private OnColorBlobDetectListener m_maze3Listener = new OnColorBlobDetectListener() {
		boolean enabled = true;

		@Override
		public boolean isEnabled() {
			return enabled;
		}

		@Override
		public void setEnabled(boolean enabled) {
			this.enabled = enabled;
		}
		
		public void onDetected(final boolean present, final ColorBlobInfo info,
				final int length, final int width) {
			m_maze3Detector.onDetect(present, info, length, width);
		}
	};

	@Override
	protected void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		setContentView(R.layout.activity_test_sequence);
		Intent intent = getIntent();
		m_gameStage = GameStage.values()[intent.getIntExtra("stage",
				GameStage.Finals.ordinal())];

		m_mazeCameraView = (MazeCameraView) this
				.findViewById(R.id.maze_camera_view);
		m_mazeCameraView.setTeachMode(false);
		
		m_mazeCameraView.setOrientationChangeListener(m_orientationListener);
		m_mazeCameraView.setTreasureListener(m_treasureListener);
		//m_mazeCameraView.setObstacleListener(m_obstacleListener);
		m_mazeCameraView.setMaze1Listener(m_maze1Listener);
		m_mazeCameraView.setMaze2Listener(m_maze2Listener);
		m_mazeCameraView.setMaze3Listener(m_maze3Listener);
		m_mazeCameraView
				.setMazeEntranceGuideListener(m_mazeEntranceGuideListener);
		m_mazeCameraView.setOuterWallListener(m_outerWallListener);
		m_mazeCameraView.setDropAreaListener(m_dropAreaListener);

		SerialParameters params = new SerialParameters();
		params.setPortName("COM1");
		params.setBaudRate(19200);
		params.setDatabits(8);
		params.setParity("None");
		params.setStopbits(1);
		params.setEncoding("ascii");
		params.setEcho(false);

		UsbManager usb = (UsbManager) getSystemService(Context.USB_SERVICE);
		m_modbusMaster = new ModbusSerialClient(usb);

		final ScrollView sv = (ScrollView) findViewById(R.id.scrollview_log);
		TextView tv = (TextView) findViewById(R.id.text_log);
		tv.addTextChangedListener(new TextWatcher() {

			@Override
			public void afterTextChanged(Editable arg0) {
				sv.fullScroll(ScrollView.FOCUS_DOWN);
			}

			@Override
			public void beforeTextChanged(CharSequence s, int start, int count,
					int after) {
				// TODO Auto-generated method stub

			}

			@Override
			public void onTextChanged(CharSequence s, int start, int before,
					int count) {
				// TODO Auto-generated method stub

			}

		});

		setupButtons();
	}

	@Override
	public boolean onCreateOptionsMenu(Menu menu) {
		// Inflate the menu; this adds items to the action bar if it is present.
		getMenuInflater().inflate(R.menu.test_sequence, menu);
		return true;
	}

	@Override
	public void onStart() {
		super.onStart();

		try {
			m_executor = Executors.newFixedThreadPool(1);

			double width = SettingsActivity.getRobotWidth(this);
			double length = SettingsActivity.getRobotLength(this);

			RobotSensors sensors = new RobotSensors(m_modbusMaster);
			RobotCommander commander = new RobotCommander(m_modbusMaster);
			m_treasureDetector = new TreasureDetector(m_treasureListener);
			//m_obstacleDetector = new ObstacleDetector(m_obstacleListener);
			m_maze1Detector = new Maze1Detector(m_maze1Listener);
			m_maze2Detector = new Maze2Detector(m_maze2Listener);
			m_maze3Detector = new Maze3Detector(m_maze3Listener);
			m_mazeEntranceGuideDetector = new MazeEntranceGuideDetector(m_mazeEntranceGuideListener);
			m_outerWallDetector = new OuterWallDetector(m_outerWallListener);
			m_dropAreaDetector = new DropAreaDetector(m_dropAreaListener);
			
			RobotConfig config = new RobotConfig();
			config.width = width;
			config.length = length;
			config.sensors = sensors;
			config.commander = commander;
			config.predefinedMazeSolver = new LeftFirstMazeSolver();
			config.treasureDetector = m_treasureDetector;
			//config.obstacleDetector = m_obstacleDetector;
			config.maze1Detector = m_maze1Detector;
			config.maze2Detector = m_maze2Detector;
			config.maze3Detector = m_maze3Detector;
			config.dropAreaDetector = m_dropAreaDetector;
			config.mazeEntranceGuideDetector = m_mazeEntranceGuideDetector;
			config.outerWallDetector = m_outerWallDetector;

			m_robot = new Robot(config, m_gameStage);

			m_robot.setLogger(m_logger, 5);
			m_robot.start(false);
		} catch (Exception ex) {
			Toast.makeText(this, ex.getMessage(), Toast.LENGTH_SHORT).show();
		}
	}

	@Override
	public void onStop() {
		m_robot.reset();
		super.onStop();
		m_mazeCameraView.onDeactivated();
	}

	@Override
	public void onResume() {
		try {
			m_modbusMaster.connect("COM1");
		} catch (Exception e) {
			Toast.makeText(this, "Failed to connect to the modbus slave",
					Toast.LENGTH_LONG).show();
		}

		super.onResume();
		m_mazeCameraView.onActivated();
	}

	@Override
	public void onPause() {
		m_modbusMaster.disconnect();
		super.onPause();
		m_mazeCameraView.onDeactivated();
	}

	private void setupButtons() {
		Button btn = (Button) findViewById(R.id.button_find_treasure);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testFindTreasure(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_approach_treasure);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testApproachTreasure(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_pick_treasure);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testPickTreasure(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_find_maze);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testFindMaze(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_approach_maze);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testApproachMaze(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_find_maze_entrance);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testFindMazeEntrance(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_enter_maze);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testEnterMaze(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_find_drop_area);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testFindDropArea(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_drop_treasure);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testDropTreasure(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_exit_maze);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testExitMaze(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_test_maze3);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				clearLog();
				
				RobotTask robotTask = new RobotTask(TestSequenceActivity.this,
						new Runnable() {

							@Override
							public void run() {
								m_robot.testMaze3(TestSequenceActivity.this);
							}
						});

				robotTask.executeOnExecutor(m_executor);
			}

		});

		btn = (Button) findViewById(R.id.button_stop);

		btn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				m_shouldStop = true;
			}

		});
	}

	private void clearLog() {
		TextView logDisplay = (TextView) findViewById(R.id.text_log);
		logDisplay.setText("");
	}
	
	private void enableButtons(boolean b) {
		m_shouldStop = false;

		Button btn = (Button) findViewById(R.id.button_find_treasure);
		btn.setEnabled(b);

		btn = (Button) findViewById(R.id.button_approach_treasure);
		btn.setEnabled(b);

		btn = (Button) findViewById(R.id.button_pick_treasure);
		btn.setEnabled(b);

		btn = (Button) findViewById(R.id.button_find_maze);
		btn.setEnabled(b);

		btn = (Button) findViewById(R.id.button_approach_maze);
		btn.setEnabled(b);

		btn = (Button) findViewById(R.id.button_find_maze_entrance);
		btn.setEnabled(b);

		btn = (Button) findViewById(R.id.button_enter_maze);
		btn.setEnabled(b);

		btn = (Button) findViewById(R.id.button_find_drop_area);
		btn.setEnabled(b);

		btn = (Button) findViewById(R.id.button_drop_treasure);
		btn.setEnabled(b);

		btn = (Button) findViewById(R.id.button_exit_maze);
		btn.setEnabled(b);
	}

	@Override
	public boolean shouldStop() {
		return m_shouldStop;
	}

	static class RobotTask extends AsyncTask<Void, Void, String> {
		TestSequenceActivity m_activity;
		Runnable m_task;

		public RobotTask(TestSequenceActivity activity, Runnable task) {
			m_activity = activity;
			m_task = task;
		}

		@Override
		protected void onPreExecute() {
			m_activity.enableButtons(false);
		}

		@Override
		protected String doInBackground(Void... params) {
			try {
				m_task.run();

				return "Done";
			} catch (Exception ex) {
				return "Failed. " + ex.toString();
			}
		}

		@Override
		protected void onPostExecute(String result) {
			m_activity.enableButtons(true);

			//Toast.makeText(m_activity, result, Toast.LENGTH_LONG).show();
			m_activity.m_logger.log(result);
		}
	}
}
